#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <quadrotor_msgs/TakeoffLand.h>
#include <mavros_msgs/RCIn.h>

double pos[3]={0,0,0};//定义一个矩阵来存储当前的xyz位置；
void pose_cb(const nav_msgs::Odometry::ConstPtr &msg)
    {
        pos[0]=msg->pose.pose.position.x;
        pos[1]=msg->pose.pose.position.y;
        pos[2]=msg->pose.pose.position.z;//回调订阅函数，存储到已定义的pos[3]
    }

int main(int argc,char **argv)
{           
    ros::init(argc, argv,"pub_goal_raw");//节点名称
    ros::NodeHandle nh;// 创建节点句柄

    ros::Subscriber fast_sub = nh.subscribe<nav_msgs::Odometry> ("/Odometry", 1, pose_cb);//回调
    //订阅节点
    ros::Publisher goal_pub = nh.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 10);
    ros::Publisher takeland_pub = nh.advertise<quadrotor_msgs::TakeoffLand>("/px4ctrl/takeoff_land", 10);
    //发布节点
    
    ros::Rate rate(20.0);//生成一个频率对象，rate
    setlocale(LC_ALL,"");//中文
    
    ros::Duration(1.0).sleep();
    // 等待发布者准备好
    
    //设置quadrotor_msgs::TakeoffLand goal消息包
    quadrotor_msgs::TakeoffLand takeland;
    takeland.takeoff_land_cmd=1;
    //设置quadrotor_msgs::TakeoffLand goal消息包
    takeland_pub.publish(takeland);

    ros::Time last_request = ros::Time::now();

    while (ros::ok())
    {
        if(abs(pos[2]-1.2)<0.05 && ros::Time::now()-last_request > ros::Duration(8.0)){
            ROS_INFO("到达指定高度，跳出循环");
            break;//到达起飞高度0.6m
        }
        ros::spinOnce();
        rate.sleep();//用于控制频率
    }

    for(int i = 10; ros::ok() && i > 0; --i)
    {
    ros::spinOnce();//调用回调读取位置
    rate.sleep();
    }

    double goal_1[3]={0};
    double goal_2[3]={0};
    double goal_3[3]={0};
    double goal_4[3]={0};
    double goal_5[3]={0};

//设置geometry_msgs::PoseStamped goal消息包
    geometry_msgs::PoseStamped goal;
    goal.header.frame_id = "world"; //设置目标点的坐标系
    goal.header.stamp = ros::Time::now();
    goal.pose.position.x = pos[0];
    goal.pose.position.y = pos[1];
    goal.pose.position.z = pos[2];
    goal.pose.orientation.w = 1.0; //w=1默认方向
    goal.pose.orientation.x = 0.0;
    goal.pose.orientation.y = 0.0;
    goal.pose.orientation.z = 0.0;
//设置geometry_msgs::PoseStamped goal消息包

    ROS_INFO("现在位置: [%.2f, %.2f, %.2f]", pos[0], pos[1], pos[2]);
    ROS_INFO("goal_1: [%.2f, %.2f, %.2f]", goal_1[0], goal_1[1], goal_1[2]);
    ROS_INFO("goal_2: [%.2f, %.2f, %.2f]", goal_2[0], goal_2[1], goal_2[2]);
    ROS_INFO("goal_3: [%.2f, %.2f, %.2f]", goal_3[0], goal_3[1], goal_3[2]);
    ROS_INFO("goal_4: [%.2f, %.2f, %.2f]", goal_4[0], goal_4[1], goal_4[2]);
    ROS_INFO("goal_5: [%.2f, %.2f, %.2f]", goal_5[0], goal_5[1], goal_5[2]);


    int go=0;
    int a=99;
    last_request = ros::Time::now();//更新时间
    
//////------------------------------------------------------------------------------------------

    while (ros::ok())
    {
        if(go==0)
        {
            goal.pose.position.x = goal_1[0];
            goal.pose.position.y = goal_1[1];
            goal.pose.position.z = goal_1[2];
            last_request = ros::Time::now();
            goal_pub.publish(goal);
            ROS_INFO("go for1: [%.2f, %.2f, %.2f]", goal_1[0], goal_1[1], goal_1[2]);
            go=1;
            a=0;
        }

        if((a==0) && abs(pos[0]-goal_1[0])<0.15 && abs(pos[1]-goal_1[1])<0.15)
        {
            ROS_INFO("到达点1");
            last_request = ros::Time::now();//到达1后更新一下时间戳
            a=1;
        }

        if((a==1) && ros::Time::now()-last_request > ros::Duration(5.0))
        {
           goal.pose.position.x = goal_2[0];
           goal.pose.position.y = goal_2[1];
           goal.pose.position.z = goal_2[2];
           last_request = ros::Time::now();
           goal_pub.publish(goal);
           ROS_INFO("go for2: [%.2f, %.2f, %.2f]", goal_2[0], goal_2[1], goal_2[2]);
           a=2;
        }
        
        if((a==2)&&abs(pos[0]-goal_2[0])<0.15 && abs(pos[1]-goal_2[1])<0.15)
        {
            ROS_INFO("到达点2");
            last_request = ros::Time::now();//到达2后更新一下时间戳
            a=3;
        }
        
        if((a==3) && ros::Time::now()-last_request > ros::Duration(5.0))
        {
           goal.pose.position.x = goal_5[0];
           goal.pose.position.y = goal_5[1];
           goal.pose.position.z = goal_5[2];
           last_request = ros::Time::now();
           goal_pub.publish(goal);
           ROS_INFO("go for3: [%.2f, %.2f, %.2f]", goal_3[0], goal_3[1], goal_3[2]);
           a=4;
        }
        
        if((a==4)&&abs(pos[0]-goal_3[0])<0.15 && abs(pos[1]-goal_3[1])<0.15)
        {
            ROS_INFO("到达点3");
            last_request = ros::Time::now();//到达3后更新一下时间戳
            a=5;
        }




        if((a==5) && ros::Time::now()-last_request > ros::Duration(5.0))//&&abs(pos[0]-goal_2[0])<0.05 && abs(pos[1]-goal_2[1])<0.05
        {
            takeland.takeoff_land_cmd=2;
            takeland_pub.publish(takeland);
            ROS_INFO("land");
            a=10;
        }
        // if((a==5) && ros::Time::now()-last_request > ros::Duration(2.0))//&&abs(pos[0]-goal_3[0])<0.05 && abs(pos[1]-goal_3[1])<0.05
        // {
        //    goal.pose.position.z = 0 ;
        //    last_request = ros::Time::now();
        //    goal_pub.publish(goal);
        //    ROS_INFO("land");
        //    a=6;
        // }
        ros::spinOnce();
        rate.sleep();//用于控制频率
    }

    return 0;
    
}

